#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"
#include "learning_interface/srv/get_point_cloud.hpp" 
#include <mutex>
#include <memory>
#include <chrono>

using namespace std::chrono_literals;

class LidarServiceNode : public rclcpp::Node {
public:
  LidarServiceNode() : Node("livox_service_node") {
    // 订阅雷达点云话题
    subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
      "/livox/lidar", rclcpp::SensorDataQoS(),
      [this](const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
        this->pointcloud_callback(msg);
      });

    // 创建服务
    service_ = this->create_service<learning_interface::srv::GetPointCloud>(
      "get_latest_pointcloud",
      [this](
        const std::shared_ptr<rmw_request_id_t> request_header,
        const std::shared_ptr<learning_interface::srv::GetPointCloud::Request> request,
        const std::shared_ptr<learning_interface::srv::GetPointCloud::Response> response) {
        this->handle_service_request(request_header, request, response);
      });

    // 初始化点云消息
    latest_pointcloud_.header.frame_id = "livox_frame";
    latest_pointcloud_.height = 1;
    latest_pointcloud_.width = 0;  // 初始化为空点云
    latest_pointcloud_.is_dense = true;
    
    RCLCPP_INFO(this->get_logger(), "雷达服务节点已启动，等待点云数据和服务请求...");
  }

private:
  // 点云回调：存储最新数据
  void pointcloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) {
    std::lock_guard<std::mutex> lock(mutex_);
    latest_pointcloud_ = *msg;
    last_received_time_ = this->now();
  }

  // 服务请求处理
  void handle_service_request(
    const std::shared_ptr<rmw_request_id_t> /*request_header*/,
    const std::shared_ptr<learning_interface::srv::GetPointCloud::Request> /*request*/,
    const std::shared_ptr<learning_interface::srv::GetPointCloud::Response> response) {
    
    std::lock_guard<std::mutex> lock(mutex_);
  
    // 检查是否有有效数据
    auto now = this->now();
    auto time_diff = now - last_received_time_;
  
    if (latest_pointcloud_.width == 0 || time_diff > 5s) {
      RCLCPP_WARN(this->get_logger(), "无可用点云数据！最后接收时间: %.1f秒前", 
                  time_diff.seconds());
    
      // 创建空点云
      sensor_msgs::msg::PointCloud2 empty_cloud;
      empty_cloud.header.stamp = now;
      empty_cloud.header.frame_id = "livox_frame";
      empty_cloud.height = 1;
      empty_cloud.width = 0;
      empty_cloud.is_dense = true;
    
      response->pointcloud = empty_cloud;
      return;
  }

  // 返回最新点云
  response->pointcloud = latest_pointcloud_;
  RCLCPP_INFO(this->get_logger(), "已响应点云数据请求 (点数: %d)", 
              latest_pointcloud_.width);
  }

  // 成员变量
  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
  rclcpp::Service<learning_interface::srv::GetPointCloud>::SharedPtr service_;
  sensor_msgs::msg::PointCloud2 latest_pointcloud_;
  rclcpp::Time last_received_time_;
  std::mutex mutex_;
};

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<LidarServiceNode>();
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}